#include <iostream>
#include "SCServo.h"

#define STEP 10

SMSBL sm;

u8 ID[6] = {0, 1, 2, 3, 4, 5};
s16 Position[6] = {2047, 2047, 2047, 2426, 2047, 2047};
u16 Speed[6] = {80, 80, 80, 80, 80, 80};
u8 ACC[6] = {100, 100, 100, 100, 100, 100};

void Reset();

struct Switch
{
    bool s[6][2];     // 6 angle, the first is move forward, the second is move backward

    inline bool operator==(const Switch &sw) const
    {
        return (sw.s == s);
    }
};

Switch sw;


int main(int argc, char **argv)
{
    // check for port
    if (argc < 2)
    {
        std::cout << "argc error!" << std::endl;
        return 0;
    }
    std::cout << "serial:" << argv[1] << std::endl;
    if (!sm.begin(1000000, argv[1]))
    {
        std::cout << "Failed to init smsbl motor!" << std::endl;
        return 0;
    }

    // back to the reset place
    Reset();
    usleep(5e6);

    std::cout << "Reset Done!" << std::endl;

    while (1)
    {
        for (int i = 0; i < 6; i++)
        {
            Position[i] += STEP;
            // if (sw.s[i][0])
            // {
            //     Position[i] += STEP;
            // }
            // else if (sw.s[i][1])
            // {
            //     Position[i] -= STEP;
            // }
        }
        
        sm.SyncWritePosEx(ID, 6, Position, Speed, ACC);

    }

}

void Reset()
{
    for (int i = 0; i < 6; i++)
    {
        Position[i] = 2047;
    }
    sm.SyncWritePosEx(ID, 6, Position, Speed, ACC);
}